#include <stdio.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <time.h>

#include "imagesource/image_source.h"
#include "common/imageconvert.h"
#include "common/image_u8.h"
#include "common/image_u32.h"
#include "common/timestamp.h"

int main(int argc, char * argv[])
{
    char *url = NULL;
    char const *outdir = NULL;

    if (argc > 1) {
        // if URL is provided on command line, use that.
        url = argv[1];
    } else {
        // otherwise, show all cameras and use the first one.

        char **urls = image_source_enumerate();
        printf("Cameras:\n");
        for (int i = 0; urls[i] != NULL; i++)
            printf("  %3d: %s\n", i, urls[i]);

        if (urls[0]==NULL) {
            printf("Found no cameras.\n");
            return -1;
        }

        url = urls[0];
    }

    if (argc > 2)
        outdir = argv[2];

    image_source_t *isrc = image_source_open(url);

    if (isrc == NULL) {
        printf("Error opening device.\n");
        return -1;
    }

    for (int i = 0; i < isrc->num_formats(isrc); i++) {
        image_source_format_t *ifmt = isrc->get_format(isrc, i);
        printf("%3d: %4d x %4d (%s)\n", i, ifmt->width, ifmt->height, ifmt->format);
    }

    int format = 0;
    isrc->set_format(isrc, format);

    if (1) {
        int res = isrc->start(isrc);
        printf("start: res = %d\n", res);
    }

    int nframes = 0;

//    setlinebuf(stdout);
    image_source_format_t *ifmt = isrc->get_format(isrc, format);
    image_u32_t * rgb_full = image_u32_create(ifmt->width, ifmt->height);
    image_u32_t * rgb_half = image_u32_create(ifmt->width/2, ifmt->height/2);
    image_u8_t * gray_full = image_u8_create(ifmt->width, ifmt->height);

    uint64_t last_end= 0;
    while(1) {
        void *imbuf = NULL;
        int imbuflen = 0;


        int res = isrc->get_frame(isrc, &imbuf, &imbuflen);
        if (res < 0) {
            printf("get_frame fail: %d\n", res);
            continue;
        } else {
            nframes++;
        }

        // Step 0: Convert byte buffer to image_u8_t
        image_u8_t * bayer = malloc(sizeof(image_u8_t));
        bayer->width = ifmt->width;
        bayer->height = ifmt->height;
        bayer->stride = ifmt->width*2; // two bytes per pixel
        bayer->buf = (uint8_t*)imbuf;

        int64_t start = timestamp_now();
        // Step 1: Debayer
        debayer_GBRG(bayer, rgb_full);
        debayer_GBRG_half(bayer, rgb_half);
        image_u8_fill_gray_from_rgb_u32(gray_full,rgb_full);

        if (outdir != NULL) {
            char  filename[256];
            sprintf(filename,"%s/full.ppm",outdir);
            image_u32_write_pnm(rgb_full,filename);

            sprintf(filename,"%s/half.ppm",outdir);
            image_u32_write_pnm(rgb_half, filename);

            sprintf(filename,"%s/full.pgm",outdir);
            image_u8_write_pnm(gray_full, filename);
        }

        int64_t end = timestamp_now();

        uint64_t diff_process = (end - start);
        uint64_t diff_loop = (end - last_end);

        printf("get_frame: res = %d count = %10d (%10d bytes)  process rate: %.2f  Hz loop rate: %.2f Hz\r",
               res, nframes, imbuflen, 1E6/(diff_process),1E6/(diff_loop));
        fflush(stdout);
        isrc->release_frame(isrc, imbuf);
        last_end = end;
    }
    printf("\n");
    fflush(stdout);
    isrc->close(isrc);
    return 1;
}
